#include "mpu6050.h"
#include "delay.h"

uint8_t MPU_Init( void )
{
	uint8_t res;
	
    I2cInit(400e3);
    I2C_Enable(I2C0);
	
	MPU_Write_Byte( MPU_PWR_MGMT1_REG, 0X80 );
	delay_ms(100);
	MPU_Write_Byte( MPU_PWR_MGMT1_REG, 0X00 );
	
	MPU_Set_Gyro_Fsr( 3 );
	MPU_Set_Accel_Fsr( 0 );
	MPU_Set_Rate( 50);
	
	MPU_Write_Byte( MPU_INT_EN_REG, 0X00 );  
	
	MPU_Write_Byte( MPU_USER_CTRL_REG, 0X00 );
	MPU_Write_Byte( MPU_FIFO_EN_REG, 0X00 );  
	MPU_Write_Byte( MPU_INTBP_CFG_REG, 0X80 );
	
	
	res = MPU_Read_Byte( MPU_DEVICE_ID_REG ); 
    printf("ADDR = %02x\r\n", res);
	if( res==MPU_ADDR )
	{
		MPU_Write_Byte( MPU_PWR_MGMT1_REG, 0X01 );
		MPU_Write_Byte( MPU_PWR_MGMT2_REG, 0X00 );
		MPU_Set_Rate( 50 );
		return 0;
	}
	else
		return 1;
}

uint8_t MPU_Read_Byte( uint8_t reg )
{
	uint8_t data;
	
    I2C_StartSend(I2C0, MPU_ADDR);
	I2C_SendData(I2C0, reg);
	I2C_StartReceive(I2C0, MPU_ADDR);
	data = I2C_StopReceiveWait(I2C0);
	return data;
}


uint8_t MPU_Write_Byte( uint8_t reg, uint8_t data )
{
    I2C_StartSend(I2C0, MPU_ADDR);
	I2C_SendData(I2C0, reg);
	I2C_StopSend(I2C0, data);
	return 0;
}


uint8_t MPU_Read_Continue( uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf )
{
	I2C_StartSend(I2C0, MPU_ADDR);
    I2C_SendData(I2C0, reg);
    I2C_StartReceive(I2C0, MPU_ADDR);
	
	while( len )
	{
		if( len==1 )
			*buf = I2C_StopReceiveWait(I2C0);
		else
			*buf = I2C_ReceiveDataWait(I2C0);
		len--;
		buf++;
	}

	return 0;
}

uint8_t MPU_Write_Continue( uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf )
{
	uint8_t i;
    I2C_StartSend(I2C0, MPU_ADDR);
    I2C_SendData(I2C0, reg);
	i=0;
	while( len )
	{
		if(len>1)
		{
		  I2C_SendData(I2C0,  buf[i]);
		}
		else
		{

		  I2C_StopSend(I2C0,  buf[i]);
		}
		i++;
		len--;
	}
	return 0;
}


uint8_t MPU_Set_Gyro_Fsr( uint8_t fsr )
{
	return MPU_Write_Byte( MPU_GYRO_CFG_REG, fsr<<3 );
}

uint8_t MPU_Set_Accel_Fsr( uint8_t fsr )
{
	return MPU_Write_Byte( MPU_ACCEL_CFG_REG, fsr<<3 );
}

uint8_t MPU_Set_LPF( uint16_t lpf )
{
	uint8_t data=0;
	
	if(lpf>=188)data=1;
	else if(lpf>=98)data=2;
	else if(lpf>=42)data=3;
	else if(lpf>=20)data=4;
	else if(lpf>=10)data=5;
	else data=6; 
	return MPU_Write_Byte(MPU_CFG_REG,data);
}


uint8_t MPU_Set_Rate( uint16_t rate )
{
	uint8_t data;
	if(rate>1000)
		rate=1000;
	if(rate<4)
		rate=4;
	data=1000/rate-1;
	data=MPU_Write_Byte(MPU_SAMPLE_RATE_REG,data);	
 	return MPU_Set_LPF(rate/2);	
}

short MPU_Get_Temperature( void )
{
	uint8_t buf[2];
	uint16_t raw;
	float temp;
	
	MPU_Read_Continue( MPU_ADDR, MPU_TEMP_OUTH_REG, 2, buf );
	raw = ( ( uint16_t )buf[0]<<8 )|buf[1];
	temp = 36.53+( (double)raw )/340;
	return temp*100;
}

uint8_t MPU_Get_Gyroscope( short *gx, short *gy, short *gz )
{
	uint8_t buf[6],res;
	if( (res=MPU_Read_Continue( MPU_ADDR, MPU_GYRO_XOUTH_REG, 6, buf ))==0 )
	{
		*gx = ((uint16_t)buf[0]<<8)|buf[1];
		*gy = ((uint16_t)buf[2]<<8)|buf[3];
		*gz = ((uint16_t)buf[4]<<8)|buf[5];
	}
	
	return res;
}

uint8_t MPU_Get_Accelerometer( short *ax, short *ay, short *az )
{
	uint8_t buf[6],res;
	
	if( (res=MPU_Read_Continue( MPU_ADDR, MPU_ACCEL_XOUTH_REG, 6, buf ))==0 )
	{
		*ax = ((uint16_t)buf[0]<<8)|buf[1];
		*ay = ((uint16_t)buf[2]<<8)|buf[3];
		*az = ((uint16_t)buf[4]<<8)|buf[5];
	}
	
	return res;
}















